// ====== 头文件：ROS 与 PCL 基础 ======
#include <ros/ros.h>                                  // ROS 日志 / 节点 / 订阅发布
#include <sensor_msgs/PointCloud2.h>                  // ROS 点云消息
#include <pcl_conversions/pcl_conversions.h>          // ROS<->PCL 消息互转
#include <pcl/point_types.h>                          // PCL 点类型定义（PointXYZ 等）
#include <pcl/point_cloud.h>                          // PCL 点云容器
#include <pcl/filters/voxel_grid.h>                   // 体素下采样
#include <pcl/segmentation/sac_segmentation.h>        // RANSAC 平面分割
#include <pcl/filters/extract_indices.h>              // 按索引提取点
#include <pcl/segmentation/extract_clusters.h>        // 欧式聚类
#include <pcl/common/pca.h>                           // PCA 主成分分析

// （可选）如果你想把候选点云再发出去，用这个发布器
ros::Publisher pub;

// 订阅点云后的回调函数
void cloudCb(const sensor_msgs::PointCloud2ConstPtr& msg)
{
    ROS_INFO("pointcloudreceived");
    // 1) 将 ROS 点云消息转换为 PCL 点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromROSMsg(*msg, *cloud);
    // 至此，cloud 里就是 PCL 的点云，可用于各种 PCL 算法

    // 2) 体素下采样：减少点数、降噪，提升后续算法效率与稳定性
    pcl::VoxelGrid<pcl::PointXYZ> vg;                 // 创建体素滤波器
    vg.setInputCloud(cloud);                          // 输入点云
    vg.setLeafSize(0.05, 0.05, 0.05);                 // 体素叶子大小（单位：米），可按场景调
    vg.filter(*cloud);                                // 输出仍写回 cloud（覆盖原来）

    // 3) RANSAC 平面分割：寻找“最大一致集”的平面（光伏板通常为大平面的一部分）
    pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients); // 平面模型参数 ax+by+cz+d=0
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);         // 属于该平面的点的索引集合
    pcl::SACSegmentation<pcl::PointXYZ> seg;                       // RANSAC 分割器
    seg.setModelType(pcl::SACMODEL_PLANE);                         // 模型：平面
    seg.setMethodType(pcl::SAC_RANSAC);                            // 方法：RANSAC
    seg.setDistanceThreshold(0.02);                                // 点到平面 max 距离阈值（米）
    seg.setInputCloud(cloud);                                      // 输入
    seg.segment(*inliers, *coeff);                                 // 输出 inliers（平面内点）和 coeff

    // 4) 按索引提取“平面内点”（只保留平面上的点做后续聚类/PCA）
    pcl::ExtractIndices<pcl::PointXYZ> ex;                         // 索引提取器
    ex.setInputCloud(cloud);                                       // 输入原点云
    ex.setIndices(inliers);                                        // 指明要提取的点索引
    pcl::PointCloud<pcl::PointXYZ>::Ptr plane(new pcl::PointCloud<pcl::PointXYZ>());
    ex.filter(*plane);                                             // 输出：plane（平面点云）

    // 5) 在平面里做“欧式聚类”：把同一平面上的不同物体块分开（每块候选做一次 PCA）
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>()); // KdTree 索引
    tree->setInputCloud(plane);                                    // 索引目标：平面点云

    std::vector<pcl::PointIndices> clusters;                       // 聚类结果（每个聚类是一组点索引）
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;             // 欧式聚类器
    ec.setClusterTolerance(0.2);                                   // 聚类“邻近”半径（米），可调
    ec.setMinClusterSize(100);                                     // 最小点数，小于则忽略
    ec.setSearchMethod(tree);                                      // 使用上面的 KdTree
    ec.setInputCloud(plane);                                       // 输入为平面点
    ec.extract(clusters);                                          // 输出多个 cluster（每个是一个候选块）

    // 6) 对每个 cluster 做 PCA，得到 length/width/thickness，并计算 ratio/area
    for (auto &c : clusters)
    {
        // 将该 cluster 的点复制出来成单独点云 cl
        pcl::PointCloud<pcl::PointXYZ>::Ptr cl(new pcl::PointCloud<pcl::PointXYZ>());
        cl->reserve(c.indices.size());
        for (auto i : c.indices)
            cl->push_back(plane->points[i]);

        // 对该块做 PCA（内部会求协方差 → 特征值/特征向量）
        pcl::PCA<pcl::PointXYZ> pca;
        pca.setInputCloud(cl);

        // 取主轴方向上的特征值（注意：这里返回的数值与“真实物理长度”比例相关，
        // 在极简示例里我们直接用它们近似代表 length/width/thickness 的相对量纲）
        auto eval = pca.getEigenValues(); // eval[0] >= eval[1] >= eval[2]（大致理解为“方差”大小）
        float thickness = eval[2];        // 最小主轴，对应“厚度方向”的方差（越小越“薄平”）
        float width     = eval[1];        // 次主轴，对应“宽度方向”方差
        float length    = eval[0];        // 最大主轴，对应“长度方向”方差

        // 基于特征值的比值做几何规则（极简近似）：
        float ratio = length / width;     // 长宽比（光伏板常在 1.2~2.2）
        float area  = length * width;     // 近似面积（只用于相对筛选，非真实 m²）

        // 7) 几何阈值：筛“像光伏板”的候选块（把数值当相对量用，足以做“规则过滤”）
        if (ratio > 1.2 && ratio < 2.2 )
        {
            ROS_WARN("==== Solar Panel Candidate Found! ====");
            ROS_INFO("ratio=%.2f area=%.2f thickness=%.2f pts=%zu",
                     ratio, area, thickness, cl->size());
        }
    }

    // （可选）如果你想发布“平面点”或“候选点”，在这里把 plane/cl 发出去
    // sensor_msgs::PointCloud2 out; pcl::toROSMsg(*plane, out); out.header = msg->header; pub.publish(out);
}

// 主函数：初始化 ROS，订阅点云，spin
int main(int argc, char** argv)
{
    ros::init(argc, argv, "simple_panel_pca");        // 初始化节点名
    ros::NodeHandle nh;                               // 句柄

    // 订阅点云话题（换成你真实的点云话题名）
    ros::Subscriber sub = nh.subscribe("/camera/depth/points", 1, cloudCb);

    // （可选）发布筛选后的点云
    pub = nh.advertise<sensor_msgs::PointCloud2>("panel_candidates", 1);

    ros::spin();                                      // 循环等待回调
    return 0;
}